/**
 * ===========================================================================
 * File:   color-convert.c
 * Author: gustavo
 *
 * Created on October 25, 2010, 6:59 PM
 * ---------------------------------------------------------------------------
 *
 *  Copyright (C) 2010  Gustavo Teodoro Laureano
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * ===========================================================================
 **/
#include <stdlib.h>
#include "color-convert.h"
#include "../../util/macros.h"

#define YUV2R(y, u, v) ({ int r = (y) + ((((v)-128)*1436) >> 10); r > 255 ? 255 : r < 0 ? 0 : r; })
#define YUV2G(y, u, v) ({ int g = (y) - ((((u)-128)*352 + ((v)-128)*731) >> 10); g > 255 ? 255 : g < 0 ? 0 : g; })
#define YUV2B(y, u, v) ({ int b = (y) + ((((u)-128)*1814) >> 10); b > 255 ? 255 : b < 0 ? 0 : b; })
#define CLIP(color) (unsigned char)(((color)>0xFF)?0xff:(((color)<0)?0:(color)))
/* */
void image_rgb24_to_gray8( int width, int height, const unsigned char * rgb, unsigned char * gray ) {
	float px;
	int row;
	int col;

	const unsigned char * rgb_data = rgb;
	unsigned char * gray_data = gray;

	for (row = 0; row < height; row++) {
		for (col = 0; col < width; col++) {
			/*
			 idx = (row * width + col)*3;
						px = 0.2125 * rgb[idx];
						px += 0.7154 * rgb[idx + 1];
						px += 0.0721 * rgb[idx + 2];
			 */
			px = 0.2125 * (*rgb_data++);
			px += 0.7154 * (*rgb_data++);
			px += 0.0721 * (*rgb_data++);

			*gray_data = (unsigned char) px;
			gray_data++;
		}
	}
}
void image_rgb_to_hsv( unsigned char * rgb, float * hsv, int width, int height ) {

	int row;
	int col;
	int idx;

	float M, m;
	float C;

	float R, G, B;
	float H, S, V;
	
	M = 0;
	m = 0;

	for (row = 0; row < height; row++) {
		for (col = 0; col < width; col++) {
			idx = (row * width + col)*3;

			/* R, G e B devem estar no intervalo [0,1] */
			R = ((float) rgb[idx]) / 255.0;
			G = ((float) rgb[idx + 1]) / 255.0;
			B = ((float) rgb[idx + 2]) / 255.0;

			// calculando Chroma
			if (R >= G && R >= B) {
				M = R;
				if (G >= B) m = B;
				else m = G;
			}
			if (G >= R && G >= B) {
				M = G;
				if (R >= B) m = B;
				else m = R;
			}
			if (B >= R && B >= G) {
				M = B;
				if (R >= G) m = G;
				else m = R;
			}
			C = M - m; // chroma

			// definindo H
			if (C == 0) {
				H = 0;
			} else {
				if (M == R) {
					H = (int) ((G - B) / C) / 6.0;
				} else {
					if (M == G) {
						H = ((B - R) / C) + 2.0;
					} else {
						// M == B
						H = ((R - G) / C) + 4.0;
					}
				}
			}
			H = (H * 60.0) / 359.0;

			// definindo V
			V = M;

			// definindo S
			if (C == 0) S = 0;
			else S = C / V;

			hsv[idx] = H;
			hsv[idx + 1] = S;
			hsv[idx + 2] = V;
		}
	}
}
void image_hsv_to_rgb( float * hsv, unsigned char * rgb, int width, int height ) {

	int row;
	int col;
	int idx;

	float R, G, B;
	float H, S, V;
	float C, X, Hl, m;

	// <editor-fold defaultstate="collapsed" desc="converting each pixel">
	for (row = 0; row < height; row++) {
		for (col = 0; col < width; col++) {
			idx = (row * width + col)*3;

			/* H deve estar no intervalo [0,360)
			 * S, V devem estar no intervalo [0,1] */
			H = (float) hsv[idx] * 359.0;
			S = (float) hsv[idx + 1];
			V = (float) hsv[idx + 2];

			C = V * S;
			Hl = H / 60;
			X = C * (1.0 - ABS( (int) (Hl / 2.0) - 1.0 ));

			if (H < 0) {

			} else {
				if (Hl < 1) {
					R = C;
					G = X;
					B = 0;
				} else {
					if (Hl < 2) {
						R = X;
						G = C;
						B = 0;
					} else {
						if (Hl < 3) {
							R = 0;
							G = C;
							B = X;
						} else {
							if (Hl < 4) {
								R = 0;
								G = X;
								B = C;
							} else {
								if (Hl < 5) {
									R = X;
									G = 0;
									B = C;
								} else {
									R = C;
									G = 0;
									B = X;
								}
							}
						}
					}
				}
			}
			m = V - C;
			rgb[idx] = (R + m) * 255;
			rgb[idx + 1] = (G + m) * 255;
			rgb[idx + 2] = (B + m) * 255;
		}
	}// </editor-fold>
}
void image_hsv_to_view( float * hsv, unsigned char * view, int width, int height ) {

	int row;
	int col;
	int idx;

	// <editor-fold defaultstate="collapsed" desc="converting each pixel">
	for (row = 0; row < height; row++) {
		for (col = 0; col < width; col++) {
			idx = (row * width + col)*3;

			/* H deve estar no intervalo [0,360]
			 * S, V devem estar no intervalo [0,1] */

			//view[idx] = hsv[idx] * 255 / 360; // H - > R
			view[idx] = hsv[idx] * 255; // H - > R
			view[idx + 1] = hsv[idx + 1] * 255; // S -> G
			view[idx + 2] = hsv[idx + 2] * 255; // V -> B
		}
	}// </editor-fold>
}
void _image_yuv420p_to_gray( const unsigned char * yuv, unsigned char * gray, int width, int height ) {
	int w, h, i, _i;

	for (h = 0; h < height; h++) {

		_i = h * width;

		for (w = 0; w < width; w++) {
			i = _i + w;
			gray[i] = yuv[i];
		}
	}
}
void old_image_yuv420p_to_rgb24( const unsigned char * src, unsigned char * dest, int width, int height, int yvu ) {
	int i, j;

	const unsigned char *ysrc = src;
	const unsigned char *usrc, *vsrc;

	if (yvu) {
		vsrc = src + width * height;
		usrc = vsrc + (width * height) / 4;
	} else {
		usrc = src + width * height;
		vsrc = usrc + (width * height) / 4;
	}

	for (i = 0; i < height; i++) {
		for (j = 0; j < width; j += 2) {

#if 1	// fast slightly less accurate multiplication free code
			int u1 = (((*usrc - 128) << 7) + (*usrc - 128)) >> 6;
			int rg = (((*usrc - 128) << 1) + (*usrc - 128) +
					((*vsrc - 128) << 2) + ((*vsrc - 128) << 1)) >> 3;
			int v1 = (((*vsrc - 128) << 1) + (*vsrc - 128)) >> 1;

			*dest++ = CLIP( *ysrc + v1 );
			*dest++ = CLIP( *ysrc - rg );
			*dest++ = CLIP( *ysrc + u1 );
			ysrc++;

			*dest++ = CLIP( *ysrc + v1 );
			*dest++ = CLIP( *ysrc - rg );
			*dest++ = CLIP( *ysrc + u1 );
#else
			*dest++ = YUV2R( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2G( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2B( *ysrc, *usrc, *vsrc );
			ysrc++;

			*dest++ = YUV2R( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2G( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2B( *ysrc, *usrc, *vsrc );
#endif
			ysrc++;
			usrc++;
			vsrc++;
		}
		/* Rewind u and v for next line */
		if (!(i & 1)) {
			usrc -= width / 2;
			vsrc -= width / 2;
		}
	}
}
void _image_yuv420p_to_rgb24( const unsigned char * yuv, unsigned char * rgb, int width, int height, int yvu_flag ) {
	int i, j;

	const unsigned char * src = yuv;
	unsigned char * dest = rgb;

	const unsigned char *ysrc = src;
	const unsigned char *usrc, *vsrc;

	if (yvu_flag) {
		vsrc = src + width * height;
		usrc = vsrc + (width * height) / 4;
	} else {
		usrc = src + width * height;
		vsrc = usrc + (width * height) / 4;
	}

	for (i = 0; i < height; i++) {
		for (j = 0; j < width; j += 2) {

#if 1	// fast slightly less accurate multiplication free code
			int u1 = (((*usrc - 128) << 7) + (*usrc - 128)) >> 6;
			int rg = (((*usrc - 128) << 1) + (*usrc - 128) +
					((*vsrc - 128) << 2) + ((*vsrc - 128) << 1)) >> 3;
			int v1 = (((*vsrc - 128) << 1) + (*vsrc - 128)) >> 1;

			*dest++ = CLIP( *ysrc + v1 );
			*dest++ = CLIP( *ysrc - rg );
			*dest++ = CLIP( *ysrc + u1 );
			ysrc++;

			*dest++ = CLIP( *ysrc + v1 );
			*dest++ = CLIP( *ysrc - rg );
			*dest++ = CLIP( *ysrc + u1 );
#else
			*dest++ = YUV2R( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2G( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2B( *ysrc, *usrc, *vsrc );
			ysrc++;

			*dest++ = YUV2R( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2G( *ysrc, *usrc, *vsrc );
			*dest++ = YUV2B( *ysrc, *usrc, *vsrc );
#endif
			ysrc++;
			usrc++;
			vsrc++;
		}
		/* Rewind u and v for next line */
		if (!(i & 1)) {
			usrc -= width / 2;
			vsrc -= width / 2;
		}
	}
}



void image_yuv420p_to_gray( Image * YUV, Image ** Gray ) {
	int w, h, i, _i;

	unsigned char * yuv;
	unsigned char * gray;
	int width;
	int height;
	
	_verify(Gray);
	if(*Gray==NULL) *Gray = image_new(YUV->width, YUV->height, IMAGE_PALETTE_GRAY);
	
	yuv = YUV->data;
	gray = (*Gray)->data;
	
	width = YUV->width;
	height = YUV->height;
	
	for (h = 0; h < height; h++) {

		_i = h * width;

		for (w = 0; w < width; w++) {
			i = _i + w;
			gray[i] = yuv[i];
		}
	}
}